#include "robot_mgr.h"
#include "axisgroup_robot.h"
#include "log/zlog.hh"

using namespace axisgroup;
int AxisGroupMgr::add_axisgroup(std::shared_ptr<Robot> group, std::shared_ptr<rtdev::RtRobot> rtrobot)
{
    group->group_id_ = axis_group_.size();
    if (group->boot_init(axis_group_.size(), rtrobot))  // boot_init 后机器人获得序列号（配置文件中读取）
    {
        zlog()->error("[AxisGroupMgr] robot{} boot failed", group->group_id_);
        return -1;
    }

    bool find = false;
    for (auto& [id, ag] : axis_group_)
    {
        if (ag->get_serial_num() == group->get_serial_num())
        {
            find = true;
            break;
        }
    }
    if (find)
    {
        zlog()->error("[AxisGroupMgr] serial_num={} already exist", group->get_serial_num());
        return -1;
    }

    axis_group_.emplace(axis_group_.size(), group);
    auto jids = group->get_joint_ids();

    std::string jids_str = "[ ";
    for (auto jid : jids) { jids_str += std::to_string(jid) + " "; }
    jids_str += "]";
    zlog()->info("[AxisGroupMgr] add_axisgroup serial_num={} group_id={} joint={}", group->get_serial_num(), group->group_id_, jids_str);
    return 0;
}

int AxisGroupMgr::del_axisgroup(uint8_t group_id)
{
    if (axis_group_.count(group_id) == 0)
    {
        return -1;
    }
    axis_group_.erase(group_id);
    return 0;
}

std::shared_ptr<Robot> AxisGroupMgr::get_axisgroup(uint8_t group_id)
{
    if (axis_group_.count(group_id) == 0)
    {
        return nullptr;
    }
    return axis_group_[group_id];
}

void AxisGroupMgr::reset() { axis_group_.clear(); }

size_t axisgroup::AxisGroupMgr::group_num() { return axis_group_.size(); }